#include "speed_pid.h"
//#include "foc.h"

SPEED_PID_DEF speed_pid;         
          
			
static float Limit_Sat( float fdata,float fmax, float fmin) 
{
    fdata = (fdata > fmax) ? fmax : fdata;
    fdata = (fdata < fmin) ? fmin : fdata;

    return fdata;
}
			

void  Speed_pid_c(SPEED_PID_DEF* speed_pid_temp)
{
		//Normal PI Controller
    speed_pid_temp->error = speed_pid_temp->ref - speed_pid_temp->fbk;
   speed_pid_temp->up  = speed_pid_temp->Kp * speed_pid_temp->error;
    speed_pid_temp->I_Sum= Limit_Sat(
															speed_pid_temp->I_Sum  +  speed_pid_temp->Ki  *speed_pid_temp->error, 
                              speed_pid_temp->Max_Output, 
                              speed_pid_temp->Min_Output);
   speed_pid_temp ->output = Limit_Sat(
                              speed_pid_temp->up + speed_pid_temp->I_Sum, 
                              speed_pid_temp->Max_Output, 
                              speed_pid_temp->Min_Output);
}

void speed_pid_init(float motor_J, float motor_flux, unsigned int Pole, unsigned int N)
{
	
	//Normal PI Controller
	speed_pid.Kp = 100.0f * 6.28f * motor_J / (1.5f * Pole * motor_flux) * 12.0f;  //2; //* 4.0f;
	speed_pid.Ki = 100.0f * 6.28f * speed_pid.Kp * 0.00005f * N *0.1f;  //2;    // *0.1f;
	 
	speed_pid.Max_Output =  10.0f;   
	speed_pid.Min_Output = -5.0f;
	speed_pid.up = 0.0f;
	speed_pid.I_Sum = 0.0f;
}
//void Re_speed_init(void)
//{
//		//Series type PI Controller
////	speed_pid.Ki = 0.1f;
////	speed_pid.Kp = 0.3f;
//	
//	//Normal PI Controller
//	speed_pid.Kp = 100.0f * 6.28f * motor.J / (1.5f * Pole * motor.flux) * 4.0f;  ///2; //* 4.0f;
//	speed_pid.Ki = 100.0f * 6.28f * speed_pid.Kp * 0.00005f * N *0.1f; ///2 ;//0.1f;
//	
//	speed_pid.Max_Output = 2.0f;
//	speed_pid.Min_Output = -70.0f;
//	speed_pid.up = 0.0f;
//	speed_pid.I_Sum = 0.0f;
//}

